#! /usr/bin/env python
#coding=utf-8
import rospy
import actionlib
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
rospy.init_node('subrviz')
aa={}
goal={}
x=1
X=1
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()

'''goal[0] = MoveBaseGoal()
goal[0].target_pose.pose.position.x = 0
goal[0].target_pose.pose.position.y = 0
goal[0].target_pose.pose.position.z = 0
goal[0].target_pose.pose.orientation.x = 0
goal[0].target_pose.pose.orientation.y = 0
goal[0].target_pose.pose.orientation.z = 0
goal[0].target_pose.pose.orientation.w = 0'''

def act():
    global x
    global u
    with open('/home/xbot/catkin_ws/src/megatron/src/include/data.txt','r') as f:
        c = f.read()
    if c=='1':
        print('\nGeneral: He is here...Stop navigation and get prepared!\n')
        return
    global X
    if X<=u:
        global aa
        aa = rospy.get_param('point'+str(X))
        goal[X]=MoveBaseGoal()
        goal[X].target_pose.header.frame_id="map"
        goal[X].target_pose.pose.position.x=aa[1]
        goal[X].target_pose.pose.position.y=aa[2]
        goal[X].target_pose.pose.position.z=aa[3]
        goal[X].target_pose.pose.orientation.x=aa[4]
        goal[X].target_pose.pose.orientation.y=aa[5]
        goal[X].target_pose.pose.orientation.z=aa[6]
        goal[X].target_pose.pose.orientation.w=aa[7]
        X+=1
    client.send_goal(goal[x])
    client.wait_for_result()
    if x==X-1:
        x=0
    x+=1
    if not rospy.is_shutdown():
        act()
if __name__ == '__main__':
    with open('/home/xbot/catkin_ws/src/megatron/src/include/data.txt','w') as f:
        f.write('0')
    print('\nGeneral: How many goals do we have?\n')
    u=input()
    print('\n%d\n' % u)
    if rospy.has_param('point'+str(u)):
        print("\nGeneral: Start navigation!\n")
        act()
    
